#include "search.h"
#include <math.h>
#if(KEIL)
#include "SEEKFREE_MT9V03X.h"
#endif


int IMAGE_MIDDLE = 130;
int CircleNeedRun = 1;  // 运行圈数
int g_speed_carage = 0;  // 车库速度
/********************************************变量声明***************************************************/
uint8        g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT];  //图像原始数据，g_VideoImageData[m][n]代表图像的从上数第m行，从左数第n点，0为最黑，255为最白
//==============================图像数据================================//
_POINT  g_CenterPosition[MAX_POINT_NUM];
_POINT  g_LeftEdge[MAX_POINT_NUM], g_RightEdge[MAX_POINT_NUM];
int          g_CenterNum;
int          g_LeftEdgeNum, g_RightEdgeNum;
//==============================图像处理用变量==========================//
uint16       g_Start[10], g_End[10], g_SEnum;
uint8        g_SearchFlag = 1;
int          g_Cover;
uint8        g_CoverIndex[10], g_CoverNum;
//==============================控制用变量==============================//
int			g_DirectionControl = MID_VIDEO_POINT;
int			g_FormerDirectionControl = MID_VIDEO_POINT;
int			g_DirectionControlWhole = 0;
int			g_DirectionControlLine = 0;


//==============================新添的变量==============================//
bool FlagStraightLine = 0;  // 直道的标志，默认不是
bool FlagCarage = 0;  // 车库标志，默认不是
bool FlagOnlyLeftEdge = 1, FlagOnlyRightEdge = 0;  // 只有左边线，只有右边线
bool FlagLeftToCarage = 0, FlagRightToCarage = 1;  // 左右入库
bool FlagLeftOutCarage = FlagLeftToCarage, FlagRightOutCarage = FlagRightToCarage;  // 左右出库
int OnlyLeftEdge[MAX_POINT_NUM] = { 0 }, OnlyRightEdge[MAX_POINT_NUM] = { 0 };
int g_LeftEdgeEndNum = 0;
int g_RightEdgeEndNum = 0;
bool FlagCross = 0;  // 十字
bool OutCarage = 1;  // 出库
bool InCarage = 0;  // 入库
_POINT CarageOutLeftPoint, CarageOutRightPoint;
_POINT InflectionPointLeftUp, InflectionPointLeftDown, InflectionPointRightUp, InflectionPointRightDown;  // 四个拐点坐标
int NumZbraBlockings = 0;  // 斑马线块数目
int SumWihteBlocks = 0;  // 中心连通白块数目
bool FlagFarWingsThroghAllWindow = 0;  // 横穿整个图像的远弯标志

int Road_width[MAX_VIDEO_LINE];//赛道宽度
int Left_lost[MAX_VIDEO_LINE];
int Right_lost[MAX_VIDEO_LINE];//左右丢线数组,丢则置1
int Left_lost_num;
int Right_lost_num;
int Both_lost_num; //丢线数
int banma_flag;
//======================================================================//

/*********	search:处理数据即测试算法的函数 ********/
void Search(bool flag)
{
	InitVariate();		// 初始化变量

	IMAGE_MIDDLE_SET();  // 获取动态阈值，赛道反光特别严重时，阈值并不准确

	Fliter();			// 对图像进行滤波处理，去掉噪点

	CannySearch();		 // 边线检索

	if (FlagLeftOutCarage || FlagRightOutCarage)
	{
		OutOfCarage();		//	出库补线

		DirctionControl(flag);  // 方向控制，默认不需要停车
	}
	else  // 正常补线
	{
		printf("进入正常扫线模式\n");

		CharacterSearch();  // 特征检索

		GetMiddleLine();   // 获取中线

		DirctionControl(flag);  // 方向控制，默认不需要停车
	}
}

/*自适应动态阈值范围，获得一个相对最佳的阈值，但是容易受到光照不均匀的影响*/
void IMAGE_MIDDLE_SET()
{
	// 大津法
	unsigned long Amount = 0;
	unsigned long PixelBack = 0;
	unsigned long PixelshortegralBack = 0;
	unsigned long Pixelshortegral = 0;
	signed long PixelshortegralFore = 0;
	signed long PixelFore = 0;
	float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差
	signed short MinValue = 0, MaxValue = 255;
	unsigned char HistoGram[256] = { 0 };
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
			HistoGram[g_VideoImageData[i][j]]++;
	for (; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
	for (; MaxValue > MinValue && HistoGram[MaxValue] == 0 && MaxValue != MinValue; MaxValue--);
	if (MaxValue == MinValue)
	{
		IMAGE_MIDDLE = MaxValue;
	}
	else
	{
		if ((MinValue + 1) == MaxValue)
		{
			IMAGE_MIDDLE = MinValue;
		}
		else
		{
			Pixelshortegral = 0;
			SigmaB = -1;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				Amount += HistoGram[i];  // 像素点总数
				Pixelshortegral += HistoGram[i] * i;  // 灰度值总数
			}
			float tmp = Pixelshortegral;
			float average_pixel = tmp / Amount;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				PixelBack = PixelBack + HistoGram[i];  // 前景像素点数
				PixelFore = Amount - PixelBack;  // 背景像素点数
				OmegaBack = ((float)PixelBack) / Amount;  // 前景像素百分比
				OmegaFore = ((float)PixelFore) / Amount;  // 背景像素百分比
				PixelshortegralBack += HistoGram[i] * i; // 前景灰度值
				PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  // 背景灰度值
				MicroBack = ((float)PixelshortegralBack) / PixelBack;  // 前景灰度百分比
				MicroFore = ((float)PixelshortegralFore) / PixelFore;  // 背景灰度百分比
				Sigma = PixelBack * (MicroBack - average_pixel) * (MicroBack - average_pixel) + PixelFore * (MicroFore - average_pixel) * (MicroFore - average_pixel);  // 类间方差
				if (Sigma > SigmaB)
				{
					SigmaB = Sigma;
					IMAGE_MIDDLE = i;
				}
			}
		}
	}
}

/*直道或者类直道中线,此时中心白赛道贯穿整个图像上下*/
void StraightLine()
{
	g_CenterNum = g_RightEdgeNum;
	for (int i = 0; i < g_CenterNum; i++)
	{
		g_CenterPosition[i].x = g_RightEdge[i].x;
		g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
	}
}

/*方向控制,flag表示是否需要停车*/
void DirctionControl(bool flag)
{
	if (flag)
	{
		int g_speed_final = 0;
	}
	else
	{
		g_DirectionControlWhole = 0;
		g_DirectionControlLine = 0;
		for (int i = 0; i < g_CenterNum; i++)
		{
			if (g_CenterPosition[i].y >= 0 && g_CenterPosition[i].y <= MAX_VIDEO_POINT)
			{
				g_DirectionControlLine += (int)g_CenterPosition[i].x;
				g_DirectionControlWhole += (int)g_CenterPosition[i].y * g_CenterPosition[i].x;  //注意数据不要溢出
			}
		}
		if (g_DirectionControlLine > 0)
		{
			g_DirectionControl = g_DirectionControlWhole / g_DirectionControlLine;
		}
		//方向控制限定
		if (g_DirectionControl < 0)
		{
			g_DirectionControl = 0;
		}
		else if (g_DirectionControl > MAX_VIDEO_POINT)
		{
			g_DirectionControl = MAX_VIDEO_POINT;
		}
		if (ABS(g_DirectionControl - g_FormerDirectionControl) > 90)
		{
			g_DirectionControl = g_FormerDirectionControl;
		}
	}
}

/*边线提取*/
void CannySearch()
{
	g_LeftEdgeNum = 0;
	g_RightEdgeNum = 0;
	g_SearchFlag = 1;
	int i, line, white_width = 0;
	g_CenterNum = 0;
	// 搜边线
	for (line = 0; line <= MAX_VIDEO_LINE - 1; line++)
	{
		//首先 求出本行所有白块的位置
		g_SEnum = 0;  //本行的白块计数器清零
		if (g_VideoImageData[line][MAX_VIDEO_POINT - 1] > IMAGE_MIDDLE)  //大于阈值为白 小于阈值为黑
			g_Start[g_SEnum] = 0;
		for (i = MAX_VIDEO_POINT - 1; i > 0; i--)
		{
			if (g_VideoImageData[line][i] > IMAGE_MIDDLE && g_VideoImageData[line][i - 1] <= IMAGE_MIDDLE)
			{
				g_End[g_SEnum] = MAX_VIDEO_POINT - i;
				g_SEnum++;
			}
			else if (g_VideoImageData[line][i] <= IMAGE_MIDDLE && g_VideoImageData[line][i - 1] > IMAGE_MIDDLE)
			{
				g_Start[g_SEnum] = MAX_VIDEO_POINT - i - 1;
			}
			else if (g_SEnum >= 10)
			{
				break;
			}
		}
		if (g_VideoImageData[line][0] > IMAGE_MIDDLE && g_SEnum < 10)
		{
			g_End[g_SEnum] = MAX_VIDEO_POINT - 1;
			g_SEnum++;
		}
		//把最下面的那一行单独拿出来处理 做为后续处理的基础
		if (g_SearchFlag)                    //最下一行处理标志
		{
			int temp_mark = 0;
			if (g_SEnum == 0)
			{
				continue;
			}
			white_width = g_End[0] - g_Start[0];
			for (i = 1; i < g_SEnum; i++)  //直接求最宽的白块
			{
				if (g_End[i] - g_Start[i] >= white_width)
				{
					white_width = g_End[i] - g_Start[i];
					temp_mark = i;
				}
			}
			if (white_width > WIDTH)
			{
				g_SearchFlag = 0;
				//认为第一行最宽的白块便是所需白块
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
		else
		{
			if (g_SEnum == 0)
			{
				break;
			}
			for (i = 0, g_CoverNum = 0; i < g_SEnum; i++)
			{
				//覆盖关系，因为图像是一个联通区域 于是找与上一行已经找到白块相联通的白块
				g_Cover = MIN(g_End[i], g_RightEdge[g_RightEdgeNum - 1].y) - MAX(g_Start[i], g_LeftEdge[g_LeftEdgeNum - 1].y);
				if (g_Cover > -1)
				{
					g_CoverIndex[g_CoverNum] = i;
					g_CoverNum++;
				}
			}
			if (g_CoverNum == 1) //如果只有一个联通的 直接取出这个联通白块
			{
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[g_CoverIndex[0]];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[g_CoverIndex[0]];
			}
			else if (g_CoverNum == 0)  //没有联通的白块 说明图像已经处理完毕
			{
				break;                 //跳出整个图像处理的循环 进入求中心部分
			}
			else //有多个白块   需要取舍
			{
				//取舍规则：先求出本行的白块的中心，再寻找与上一行白块中心最相近的一个白块
				int temp_mark, temp_dis, temp_center, last_center;
				temp_mark = g_CoverIndex[0];
				temp_center = (g_Start[g_CoverIndex[0]] + g_End[g_CoverIndex[0]]) / 2;
				last_center = (g_LeftEdge[g_LeftEdgeNum - 1].y + g_RightEdge[g_RightEdgeNum - 1].y) / 2;
				temp_dis = ABS(last_center - temp_center);
				for (i = 1; i < g_CoverNum; i++)
				{
					temp_center = (g_Start[g_CoverIndex[i]] + g_End[g_CoverIndex[i]]) / 2;
					if (ABS(temp_center - last_center) < temp_dis)
					{
						temp_dis = ABS(temp_center - last_center);
						temp_mark = i;
					}
				}
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
		//丢线判断
		Left_lost[line] = 0;
		Right_lost[line] = 0;
		if (g_LeftEdge[line].y == 0)
			Left_lost[line] = 1;
		if (g_RightEdge[line].y == 161)
			Right_lost[line] = 1;
		//赛道宽度
		Road_width[line] = g_RightEdge[line].y - g_LeftEdge[line].y;
	}

	//=======================丢线计数========================//
	Left_lost_num = 0;
	Right_lost_num = 0;
	Both_lost_num = 0;
	for (i = 0; i < MAX_VIDEO_LINE; i++) {
		if (Left_lost[i] == 1 && Right_lost[i] == 1)
		{
			Left_lost_num += 1;
			Right_lost_num += 1;
			Both_lost_num += 1;
		}
		else if (Left_lost[i] == 1)
		{
			Left_lost_num += 1;
		}
		else if (Right_lost[i] == 1)
		{
			Right_lost_num += 1;
		}
	}
	printf("预提取已经完成\n");
}

/*单边线,一边边线为161或者0*/
/*只有一组边线，另一组边线完全丢失或者只残留一点时条用
* char EdgeType:边线数组的类型
* int StartIndex:开始计算的数组下标
*/
void OnlyOneEdge(int StartIndex, int EndIndex, char EdgeType, int Index)
{
	int move = MAX_VIDEO_POINT / 2;   // 位移大小
	int LeftWeight = 1;  // 左线权重大小
	int RightWeight = 1;  // 右线条权重大小
	if (EdgeType == 'L')  // 只有左边线
	{
		int i = StartIndex;
		while ((g_LeftEdge[i].y + move) <= 161 && i < EndIndex)
		{
			g_CenterPosition[i].x = g_LeftEdge[i].x;
			g_CenterPosition[i].y = g_LeftEdge[i].y + move * LeftWeight;
			g_CenterNum += 1;
			i++;
		}
	}
	if (EdgeType == 'R')  // 只有右边线
	{
		int i = StartIndex;
		while ((g_RightEdge[i].y - move) >= 0 && i < EndIndex)
		{
			g_CenterPosition[i].x = g_RightEdge[i].x;
			g_CenterPosition[i].y = g_RightEdge[i].y - move * RightWeight;
			g_CenterNum += 1;
			i++;
		}
	}
	// 一行中只有左边有点
	if (EdgeType == 'l' && (g_LeftEdge[Index].y + move * LeftWeight) <= 161)
	{
		g_CenterPosition[Index].x = g_LeftEdge[Index].x;
		g_CenterPosition[Index].y = g_LeftEdge[Index].y + move * LeftWeight;
		g_CenterNum += 1;
	}
	// 一行中只有右边有点
	if (EdgeType == 'r' && (g_RightEdge[Index].y - move * RightWeight) >= 0)
	{
		g_CenterPosition[Index].x = g_RightEdge[Index].x;
		g_CenterPosition[Index].y = g_RightEdge[Index].y - move * RightWeight;
		g_CenterNum += 1;
	}
}

/*初始化变量*/
void InitVariate()
{
	// 所有标志位需要复位
	FlagStraightLine = 0;  // 直道的标志，默认不是
	FlagCarage = 0;  // 车库标志，默认不是
	FlagOnlyLeftEdge = 0;   // 只有左边线
	FlagOnlyRightEdge = 0;  // 只有右边线
	FlagCross = 0;  // 十字
	FlagFarWingsThroghAllWindow = 0;  // 横穿整个图像的远弯标志

	g_LeftEdgeEndNum = 0;
	g_RightEdgeEndNum = 0;

	InflectionPointLeftDown.x = 0;
	InflectionPointLeftDown.y = 0;
	InflectionPointLeftUp.x = 0;
	InflectionPointLeftUp.y = 0;
	InflectionPointRightDown.x = 0;
	InflectionPointRightDown.y = 0;
	InflectionPointRightUp.x = 0;
	InflectionPointRightUp.y = 0;

	for (int i = 0; i < MAX_POINT_NUM; i++)
		g_CenterPosition[i].x = g_CenterPosition[i].y = g_LeftEdge[i].x = g_LeftEdge[i].y = g_RightEdge[i].x = g_RightEdge[i].y = 0;
}

/*十字*/
void StraightCross(int flag)
{
	// 边线补线
	// 四个点均存在，直接连线
	if ((InflectionPointLeftUp.x && InflectionPointLeftUp.y) && (InflectionPointRightUp.x && InflectionPointRightUp.y < 161) && (InflectionPointLeftDown.x && InflectionPointLeftDown.y) && (InflectionPointRightDown.x && InflectionPointRightDown.y < 161))
	{
		// 左边
		float LeftSlope = (InflectionPointLeftUp.y - InflectionPointLeftDown.y) / (InflectionPointLeftUp.x - InflectionPointLeftDown.x);
		float LeftIntercept = InflectionPointLeftDown.y - LeftSlope * InflectionPointLeftDown.x;
		for (int i = InflectionPointLeftDown.x; i < InflectionPointLeftUp.x; i++)
		{
			g_LeftEdge[i].x = i;
			g_LeftEdge[i].y = uint8(LeftSlope * i + LeftIntercept);
		}

		// 右边
		float RightSlope = (InflectionPointRightDown.y - InflectionPointRightUp.y) / (InflectionPointRightDown.x - InflectionPointRightUp.x);
		float RightIntercept = InflectionPointRightDown.y - RightSlope * InflectionPointRightDown.x;
		for (int i = InflectionPointRightDown.x; i < InflectionPointRightUp.x; i++)
		{
			g_RightEdge[i].x = i;
			g_RightEdge[i].y = uint8(RightSlope * i + RightIntercept);
		}
	}
	// 只有三个点的情况

	// 只有两个点的情况

	// 只有一个点的情况


	// 中线补线，可以认为是一条直道
	StraightLine();
}

/*特征提取*/
int CharacterSearch()
{
	// 提取图像特征
	// 类直道或直道,白块从第一行延伸到最后一行
	if (g_LeftEdgeNum == g_RightEdgeNum && g_LeftEdgeNum == MAX_VIDEO_LINE)
	{
		// 最小二乘法
		int LeftEdgeBreakLoop = 0;  // 左边线开始退出循环的行数
		int LoopVariate = 0;  // 循环变量
		int LeftLineCount = 0, RightLineCount = 0;  // 计数
		// 遍历左边线数组
		int left_i = 0, Right_i = 0;
		float sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0, Slope, Intercept;
		for (; LoopVariate < g_LeftEdgeNum; LoopVariate++)
		{
			sum_x += float(g_LeftEdge[LoopVariate].x);
			sum_y += float(g_LeftEdge[LoopVariate].y);
			sum_xx += float(g_LeftEdge[LoopVariate].x * g_LeftEdge[LoopVariate].x);
			sum_xy += float(g_LeftEdge[LoopVariate].x * g_LeftEdge[LoopVariate].y);
			if (g_LeftEdge[LoopVariate].y >= 0 && g_RightEdge[LoopVariate].y == 161)  // 只有左线像素
			{
				LeftLineCount++;
				OnlyLeftEdge[left_i] = LoopVariate;
				left_i++;
			}
			if (g_LeftEdge[LoopVariate].y == 0 && g_RightEdge[LoopVariate].y <= 161)  // 只有右线像素
			{
				RightLineCount++;
				OnlyRightEdge[Right_i] = LoopVariate;
				Right_i++;
			}
		}
		Slope = (sum_xy - sum_x * sum_y / g_LeftEdgeNum) / (sum_xx - sum_x * sum_x / g_LeftEdgeNum);  // 斜率
		Intercept = sum_y / g_LeftEdgeNum - Slope * (sum_x / g_LeftEdgeNum);  // 截距
		float error = 0;
		for (int i = 0; i < g_LeftEdgeNum; i++)
		{
			error += (Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y) * (Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y);
		}
		printf("Error:%f\n", error);
		if (error < 138)
		{
			FlagStraightLine = 1;  // 直道
			printf("识别到直道或者近似直道\n\n");
			return FlagStraightLine;
		}
		else  if (MAX_VIDEO_LINE - g_LeftEdgeNum <= 3)// 非直道
		{
			printf("识别到非直道且非近似直道\n");
			// 只有单边线的特殊弯道
			if (ABS(RightLineCount - g_RightEdgeNum) <= 2)  // 避免误差，选择小于2行，认为只有右边线存在
			{
				FlagOnlyRightEdge = 1;
				g_RightEdgeEndNum = g_RightEdgeNum;
				printf("识别到边线数目已满%d且只有单一的右边线\n\n", MAX_VIDEO_LINE);
				return FlagOnlyRightEdge;
			}
			if (ABS(LeftLineCount - g_RightEdgeNum) <= 2)  // 避免误差，选择小于2行，认为只有左边线存在
			{
				FlagOnlyLeftEdge = 1;
				g_LeftEdgeEndNum = g_LeftEdgeNum;
				printf("识别到边线数目已满%d且只有单一的左边线\n\n", MAX_VIDEO_LINE);
				return FlagOnlyLeftEdge;
			}
			FlagFarWingsThroghAllWindow = 1;  // 横穿整个图像的远弯标志
			return FlagFarWingsThroghAllWindow;
		}
		// 十字
		int MaxDistance = 0;
		bool FlagFindLeftUpPoint = 0, FlagFindRightUpPoint = 0, FlagFindLeftDownPoint = 0, FlagFindRightDownPoint = 0;  // 是否找到左右拐点的坐标
		for (int i = 0; i < g_LeftEdgeNum - 1; i++)
		{
			if (g_LeftEdge[i].y == 0 && g_LeftEdge[i + 1].y > 0 && !FlagFindLeftUpPoint && i > 0)  // 左边存在十字拐点
			{
				FlagFindLeftUpPoint = 1;
				// 求解左上角拐点坐标
				float Slope = (float)(g_LeftEdge[i].y - g_LeftEdge[g_LeftEdgeNum - 1].y) / (g_LeftEdge[i].x - g_LeftEdge[g_LeftEdgeNum - 1].x);
				float Intercept = float(g_LeftEdge[i].y - Slope * g_LeftEdge[i].x);
				float tmp = sqrt(Slope * Slope + 1);
				float tempDistance = 0;
				for (int j = i; j < g_LeftEdgeNum - 1; j++)
				{
					float temp = ABS(Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y) / tmp;  // 计算距离
					if (tempDistance < temp)
					{
						InflectionPointLeftUp.x = j;
						InflectionPointLeftUp.y = g_LeftEdge[j].y;
						tempDistance = temp;
					}
				}
				printf("InflectionPointLeftUp.x:%d,InflectionPointLeftUp.y:%d\n", InflectionPointLeftUp.x, InflectionPointLeftUp.y);
				break;
			}
			if (!FlagFindLeftDownPoint && g_LeftEdge[i].y == 0 && g_LeftEdge[i + 1].y > 0 && i > 0)
			{
				FlagFindLeftDownPoint = 1;
				// 求解左下角拐点坐标
				float Slope = (g_LeftEdge[0].y - g_LeftEdge[i].y) / (g_LeftEdge[0].x - g_LeftEdge[i].x);
				float Intercept = g_LeftEdge[0].y - Slope * g_LeftEdge[0].x;
				float tmp = sqrt(Slope * Slope + 1);
				float tempDistance = 0;
				for (int j = 0; j < i - 1; j++)
				{
					float temp = ABS(Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y) / tmp;  // 计算距离
					if (tempDistance < temp)
					{
						InflectionPointLeftDown.x = j;
						InflectionPointLeftDown.y = g_LeftEdge[j].y;
						tempDistance = temp;
					}
				}
				printf("InflectionPointLeftDown.x:%d,InflectionPointLeftDown.y:%d\n", InflectionPointLeftDown.x, InflectionPointLeftDown.y);
				break;
			}
			if (g_RightEdge[i].y == 161 && g_RightEdge[i + 1].y < 161 && !FlagFindRightUpPoint && FlagFindLeftDownPoint == 0 && FlagFindLeftUpPoint == 0 && i>0)  // 右边存在十字拐点
			{
				FlagFindRightUpPoint = 1;
				// 求解右下角拐点坐标
				break;
			}
			if (!FlagFindRightUpPoint && g_RightEdge[i].y == 161 && g_RightEdge[i + 1].y < 161 && FlagFindLeftDownPoint == 0 && FlagFindLeftUpPoint == 0 && FlagFindRightDownPoint == 0 && i>0)
			{
				FlagFindRightDownPoint = 1;
				// 求解右上角拐点坐标
				break;
			}
		}
		if ((InflectionPointLeftUp.x && InflectionPointLeftUp.y) || (InflectionPointRightUp.x && InflectionPointRightUp.y < 161) || (InflectionPointLeftDown.x && InflectionPointLeftDown.y) || (InflectionPointRightDown.x && InflectionPointRightDown.y < 161))
		{
			printf("识别到十字元素\n\n");
			FlagCross = 1;
			return 1;
		}
	}
	else
	{
		// 斑马线以及车库特征提取
		int LeftFlag = 0, RightFlag = 0, count = 0;
		for (int i = 0; i < g_LeftEdgeNum - 1; i++)
		{
			if (g_LeftEdge[i].y > 0 && g_LeftEdge[i + 1].y == 0 && LeftFlag == 0)
			{
				CarageOutLeftPoint.x = i;
				CarageOutLeftPoint.y = g_LeftEdge[i].y;
				LeftFlag = 1;
			}
			if (g_RightEdge[i].y < 161 && g_RightEdge[i + 1].y == 161 && RightFlag == 0)
			{
				CarageOutRightPoint.x = i;
				CarageOutRightPoint.y = g_RightEdge[i].y;
				RightFlag = 1;
			}
			if (ABS(g_LeftEdge[i].y - g_RightEdge[i].y) == 161)
			{
				count++;
			}
			if (LeftFlag && RightFlag)
				break;
		}
		// 出库
		if (NumZbraBlockings < 3 && FlagLeftOutCarage && FlagRightOutCarage)  // 识别到的斑马线数目小于预定数目，认定为已经出库，进行正常的出库
		{
			InCarage = 1;
			if (FlagLeftOutCarage)
			{
				FlagLeftToCarage = 1;
			}
			else
			{
				FlagRightToCarage = 1;
			}
		}
		// 入库

		printf("识别到车库元素\n");
	}
}

/*滤波函数*/
void Fliter()
{

}

/*获取中线*/
int GetMiddleLine()
{
	// 根据元素类型获取中线位置
	if (FlagStraightLine) // 直道
	{
		StraightLine();
		return 0;
	}
	if (FlagOnlyLeftEdge)  // 只有左单边线的未满整个屏幕的弯道
	{
		OnlyOneEdge(0, g_LeftEdgeNum, 'L', 0);
		return 0;
	}
	if (FlagOnlyRightEdge)  // 只有右单边线的未满整个屏幕的弯道
	{
		OnlyOneEdge(0, g_RightEdgeNum, 'R', 0);
		return 0;
	}
	if (FlagCross)  	// 十字
	{
		StraightCross(1);
		return 0;
	}
	if (FlagCarage)  // 车库
	{
		if (InCarage)
		{
			GoToCarage();  // 入库
		}
		if (OutCarage)
		{
			OutOfCarage();  // 出库
			return 0;
		}
	}
	if (FlagFarWingsThroghAllWindow)  // 充满整个屏幕的远弯道
	{
		return 0;
	}
}

/*出库，当斑马线低于阈值时，可以不调用这个函数了*/
void OutOfCarage()
{
	_POINT LeftOutPoint, CircleCenter, RightOutPoint;
	LeftOutPoint.x = 0;
	LeftOutPoint.y = 0;
	CircleCenter.x = 0;
	CircleCenter.y = 0;
	RightOutPoint.x = 0;
	RightOutPoint.y = 0;
	float a = 0, b = 0;  // 椭圆参数
	bool FlagLeftFind = 0, FlagRightFind = 0;
	// 找左右拐点
	for (int i = 0; i < g_LeftEdgeNum - 1; i++)
	{
		// 粗略找拐点
		if (g_LeftEdge[i].y > 0 && g_LeftEdge[i + 1].y == 0 && !FlagLeftFind && i > 10)
		{
			LeftOutPoint.x = g_LeftEdge[i].x;
			LeftOutPoint.y = g_LeftEdge[i].y;
			FlagLeftFind = 1;
			printf("LeftOutPoint.x:%d,LeftOutPoint.y:%d\n", LeftOutPoint.x, LeftOutPoint.y);
		}
		if (g_RightEdge[i].y < 161 && g_RightEdge[i + 1].y == 161 && !FlagRightFind && i > 10)
		{
			RightOutPoint.x = g_RightEdge[i].x;
			RightOutPoint.y = g_RightEdge[i].y;
			FlagRightFind = 1;
			printf("RightOutPoint.x:%d, RightOutPoint.y:%d\n", RightOutPoint.x, RightOutPoint.y);
		}
		if (FlagLeftFind && FlagRightFind)
		{
			break;
		}
	}
	a = (RightOutPoint.y - LeftOutPoint.y) / 2;  // 长轴长
	b = g_RightEdge[g_RightEdgeNum - 1].x - LeftOutPoint.x;  // 短轴长
	int move = 0;
	if (FlagLeftOutCarage)
	{
		// 补线
		CircleCenter.x = RightOutPoint.x;  // 中心点坐标
		CircleCenter.y = (g_RightEdge[RightOutPoint.x].y + g_LeftEdge[LeftOutPoint.x].y) / 2;
		for (int j = RightOutPoint.x; j < g_LeftEdgeNum; j++)
		{
			g_RightEdge[j].x = j;
			g_RightEdge[j].y = uint8(sqrt(a * a - a * a * (j - CircleCenter.x) * (j - CircleCenter.x) / (b * b)) + CircleCenter.y);
		}
		for (int j = 0; j < g_RightEdgeNum; j++)
		{
			if (j < RightOutPoint.x)
			{
				g_CenterPosition[j].x = j;
				g_CenterPosition[j].y = (g_LeftEdge[j].y + g_RightEdge[j].y) / 2;
				move = g_RightEdge[j].y - g_CenterPosition[j].y;
			}
			else
			{
				g_CenterPosition[j].x = j;
				g_CenterPosition[j].y = g_RightEdge[j].y - move;
			}
			g_CenterNum++;
		}
		FlagLeftOutCarage = 0;
		printf("左边出库补线条完毕\n\n");
	}
	if (FlagRightOutCarage)
	{
		// 补线
		CircleCenter.x = LeftOutPoint.x;  // 中心点坐标
		CircleCenter.y = (g_RightEdge[RightOutPoint.x].y + g_LeftEdge[LeftOutPoint.x].y) / 2;
		for (int j = LeftOutPoint.x; j < g_LeftEdgeNum; j++)
		{
			g_LeftEdge[j].x = j;
			g_LeftEdge[j].y = uint8(-sqrt(a * a - a * a * (j - CircleCenter.x) * (j - CircleCenter.x) / (b * b)) + CircleCenter.y);
		}
		for (int j = 0; j < g_LeftEdgeNum; j++)
		{
			if (j < LeftOutPoint.x)
			{
				g_CenterPosition[j].x = j;
				g_CenterPosition[j].y = (g_LeftEdge[j].y + g_RightEdge[j].y) / 2;
				move = ABS(g_LeftEdge[j].y - g_CenterPosition[j].y);
			}
			else
			{
				g_CenterPosition[j].x = j;
				g_CenterPosition[j].y = g_LeftEdge[j].y + move;
			}
			g_CenterNum++;
		}
		FlagRightOutCarage = 0;
		printf("右边出库补线条完毕\n\n");
	}
}

/*入库*/
void GoToCarage()
{
	// 识别拐点


	// 补线

}